function PIDControl9()
    pid = {};         % PID Control Struct
    init;

    function init()
        % Constant array describing speed-torque relationship
        pid.torques = [170,150,135,140,145,150,155,160,165,170,175,180,185,185,188,190,193,195,195,195,198,198,198,198,198,200,200,200,200,203,203,203,203,205,205,205,208,208,208,210,210,210,213,213,213,215,215,217,217,220,220,220,220,222,222,222,222,225,225,225,227,227,227,230,230,230,232,232,232,235,235,235,237,237,240,240,242,242,245,245,245,247,247,247,250,250,250,250,252,252,252,255,255,255,257,257,260,260,260,262,262,265,265,267,270,270,270,272,272,272,275,275,275,275,277,277,279,279,279,282,282,284,284,287,287,289,289,292,292,294,294,297,297,297,299,299,299,302,302,304,304,304,307,307,309,309,312,312,314,317,317,319,319,322,322,324,324,324,327,327,329,329,332,332,334,334,337,339,341,341,344,344,346,346,349,349,349,351,351,354,354,356,356,359,359,361,364,364,366,369,369,371,371,374,374,374,376,376,379,379,381,381,384,386,386,389,391,394,394,396,396,399,401,401,403,406,406,408,411,413,416,418,421,421,423,426,426,428,431,433,436,438,441,443,446,448,451,453,456,458,463,465,470,473,475,478,483,485,490,498,503,513,523,540,572,691];

        pid.s = {};       % Serial Communication Struct
        pid.vars = {};    % Program Variables Struct
        pid.vals = {};    % Read-in Peripheral Values Struct
        pid.gui = {};     % GUI Management Struct
        pid.plot = {};

        % Serial Settings...
        out = instrhwinfo('serial');
        ports = out.SerialPorts;
        str=ports(end);
        pid.s.port = sscanf(str{1}, 'COM%d');
        pid.s.baudRates = {110,300,600,1200,2400,4800,9600,14400,19200, 38400, 57600, 115200, 128000, 256000};
        pid.s.baudRateSel = 12;
        pid.s.baudRate = pid.s.baudRates{pid.s.baudRateSel};

        % Variable Settings... 
        % Present Development:
        % Bode Plot Variables.
        pid.vars.pos = 0.910/10;%3.8/10;    2.85 0.2 0.205
        pid.vars.posMax = 10;
        pid.vars.int = 0.03/10;
        pid.vars.intMax = 10;
        pid.vars.der = 0.150/10;%0.256/10;
        pid.vars.derMax = 10;
        pid.vars.acc = 0.001;
        
        pid.vars.motorPosDer = 0.00005;%65;
        pid.vars.motorAcc = 0%0.00005;%0.0005
        
        pid.vars.freq = 50; % Frequency of control law, hertz

        % Values read from/sent to MCU, or derived thereof.
        % Two values are stored: current value & last value.
        pid.vals.penduPos = [0 0];    % Read
        pid.vals.motorPos = [0 0];    % Read        
        pid.vals.outputpwm = [0 0];   % Write -- Signed Int -255 to 255.
        
        pid.vals.err = [0 0];
        pid.vals.errInt = [0 0];
        pid.vals.errDer = [0 0];
        
        pid.vals.motorPosInt = [0 0];
        pid.vals.motorPosDer = [0 0];
        pid.vals.motorAcc = [0 0];
        
        pid.vals.pidVal = 0; % Output Values...
        pid.vals.outputVal = [0 0];
        
        pid.vals.time = 0;
        pid.vals.backtorque = 0;
        
        % GUI Setup...
        pid.gui.f=clf;
        figure(gcf);
        pid.gui.bcl=get(pid.gui.f,'Color');
        set(pid.gui.f, 'CloseRequestFcn',@guiCloseFcn,...
            'MenuBar','none',...
            'DoubleBuffer','on',...
            'DefaultUIControlUnits','normalized',...
            'Name','Inverted Pendulum PID Controller',...
            'NumberTitle','off');
        pid.gui.portLabel = uicontrol(...
            'Style','text',...
            'String','Port:',...
            'Position',[0.05 0.93 0.05 0.04],...
            'Background',pid.gui.bcl);
        pid.gui.port = uicontrol(...
            'Style','popupmenu',...
            'Units','normalized',...
            'Position',[0.1 0.93 0.15 0.05],...
            'Tag','PortControl',...
            'CallBack',@guiPortFcn,...
            'String',{'COM1','COM2','COM3','COM4','COM5','COM6','COM7','COM8','COM9','COM10','COM11'},...
            'Value',pid.s.port);
        pid.gui.baudRateLabel = uicontrol(...
            'Style','text',...
            'String','Baud Rate:',...
            'Position',[0.26 0.93 0.1 0.04],...
            'Background',pid.gui.bcl);
        pid.gui.baudRate = uicontrol(...
            'Style','popupmenu',...
            'Units','normalized',...
            'Position',[0.37 0.93 0.15 0.05],...
            'Tag','BaudRateControl',...
            'CallBack',@guiBaudRateFcn,...
            'String',pid.s.baudRates,...
            'Value',pid.s.baudRateSel);

        pid.gui.startStop=uicontrol(...
            'Style','pushbutton',...
            'Units','normalized',...
            'Position',[0.71 0.93 0.2 0.05],...
            'Tag','StartStopControl',...
            'CallBack',@guiStartStopFcn,...
            'String','Start');
        
        
        % POSITION SLIDER ADJUSTMENT
        pos=[0.05, 0.8, 0.25, 0.05];
        pid.gui.posLabel = uicontrol(...
            'Style','Text',...
            'String','P:',...
            'Background',pid.gui.bcl,...
            'Position',[0.05 0.86 0.1 0.03]);
        pid.gui.posSleazySlide=axes('Position',pos,'Visible','off');
        pid.gui.pos=uicontrol(...
            'Style','Slider',...
            'Units','normalized',...
            'Position',pos,...
            'Tag','pos',...
            'CallBack',@guiSliderFcn,...
            'ButtonDownFcn',@sleazySliderDown,...
            'UserData',struct('name','pos',...
                              'label','P',...
                              'ax',pid.gui.posSleazySlide,...
                              'orient',(1+pos(4)>pos(3)),...
                              'CallBack',@guiSliderFcn),...
            'Value',pid.vars.pos,...
            'Min',-1,...
            'Max',1);
        pid.gui.posValue = uicontrol(...
            'Style','Edit',...
            'String','0',...
            'UserData',struct('String','P:'),...
            'Background',pid.gui.bcl,...
            'Units','Normalized',...
            'Position',[0.15, 0.86, 0.1, 0.04],...
            'Callback',{@guiSliderEdit pid.gui.pos});
        
        % INTEGRAL SLIDER ADJUSTMENT SHIT
        
        pos=[0.35, 0.8, 0.25, 0.05];
        pid.gui.intLabel = uicontrol(...
            'Style','Text',...
            'String','I:',...
            'Background',pid.gui.bcl,...
            'Position',[0.35 0.86 0.1 0.03]);
        pid.gui.intSleazySlide=axes('Position',pos,'Visible','off');
        pid.gui.int=uicontrol(...
            'Style','Slider',...
            'Units','normalized',...
            'Position',pos,...
            'Tag','int',...
            'CallBack',@guiSliderFcn,...
            'ButtonDownFcn',@sleazySliderDown,...
            'UserData',struct('name','int',...
                              'label','I',...
                              'ax',pid.gui.intSleazySlide,...
                              'orient',(1+pos(4)>pos(3)),...
                              'CallBack',@guiSliderFcn),...
            'Value',pid.vars.int,...
            'Min',-1,...
            'Max',1);
        pid.gui.intValue = uicontrol(...
            'Style','Edit',...
            'String','0',...
            'UserData',struct('String','I:'),...
            'Background',pid.gui.bcl,...
            'Units','Normalized',...
            'Position',[0.45, 0.86, 0.1, 0.04],...
            'Callback',{@guiSliderEdit pid.gui.int});
        
        
        % DERIVATIVE SLIDER ADJUSTMENT SHIT
        
        pos=[0.65, 0.8, 0.25, 0.05];
        pid.gui.derLabel = uicontrol(...
            'Style','Text',...
            'String','D:',...
            'Background',pid.gui.bcl,...
            'Position',[0.65 0.86 0.1 0.03]);
        pid.gui.derSleazySlide=axes('Position',pos,'Visible','off');
        pid.gui.der=uicontrol(...
            'Style','Slider',...
            'Units','normalized',...
            'Position',pos,...
            'Tag','der',...
            'CallBack',@guiSliderFcn,...
            'ButtonDownFcn',@sleazySliderDown,...
            'UserData',struct('name','der',...
                              'label','D',...
                              'ax',pid.gui.derSleazySlide,...
                              'orient',(1+pos(4)>pos(3)),...
                              'CallBack',@guiSliderFcn),...
            'Value',pid.vars.der,...
            'Min',-1,...
            'Max',1);
        pid.gui.derValue = uicontrol(...
            'Style','Edit',...
            'String','0',...
            'UserData',struct('String','D:'),...
            'Background',pid.gui.bcl,...
            'Units','Normalized',...
            'Position',[0.75, 0.86, 0.1, 0.04],...
            'Callback',{@guiSliderEdit pid.gui.der});

        pid.gui.plotAxes = axes(...
            'Units','Normalized',...
            'Position',[0.05 0.1 0.9 0.63]);
        title('Live Values Plot');
        pid.plot = struct(...
            'axes', pid.gui.plotAxes,...
            'xsize', 50,...
        	'x', 1:50,...
            'varsize', 2,...
            'initiated',0);
        %plotScroller(pid.plot);
        
        pid.gui.penduPosText = uicontrol('Style','text',...
            'Position',[0.8 0.63 0.1 0.07],...
            'String','Watch Here');
        
    end


    
    % GUI Callbacks:
    function guiPortFcn(src,evt)
        pid.s.port=get(src,'Value');
        %closeConnection();
        %setConnection();
    end
        
    function guiBaudRateFcn(src,evt)
        temp=get(src,'String');
        pid.s.baudRate=str2double(temp(get(src,'Value')));
        %closeConnection();
        %setConnection();
    end
        
    function guiStartStopFcn(src,evt)
        if (isequal('Start',get(src,'String')))
            % User has commanded the routine to start
            set(src,'String','Stop');
            beginRoutine();
            %if(~beginRoutine())
            %end
        else
            % User has commanded the routine to end
            endRoutine();
            set(src,'String','Start');
        end
    end

    function sleazySliderDown(src, evt)
        set(gcf, 'WindowButtonMotionFcn', {@sleazySliderMove src}, ...
            'WindowButtonUpFcn', @sleazySliderUp);
        sleazySliderMove([], [], src);
    end

    function sleazySliderMove(src, evt, slider)
        params=get(slider,'UserData');
        cp = get(params.ax, 'CurrentPoint');
        value = min([1 1; max([0 0; cp([1 3])])]);
        value = fix(100000*value) / 100000;
        maax=get(slider,'max');
        miin=get(slider,'min');
        set(slider, 'Value', value(params.orient)*(maax-miin)+miin);
        params.CallBack(slider);
    end
    function sleazySliderUp(src, evt)
        set(gcf, 'WindowButtonMotionFcn', '', ...
            'WindowButtonUpFcn', '');
    end

    function guiSliderFcn(src,evt)
        userdata = get(src, 'UserData');
        x = fix((get(src, 'Value')*pid.vars.([userdata.name 'Max']))*100000)/100000;
        pid.vars.(userdata.name) = x;
        set(pid.gui.([userdata.name 'Value']), 'String', sprintf('%.3f',x));
    end

    function guiSliderEdit(src,evt,slider)
        userdata = get(slider, 'UserData');
        miin = get(slider,'min');
        maax = get(slider,'max');
        x = max(...
            miin,...
            min(maax,...
            str2double(get(src,'String'))/(pid.vars.([userdata.name 'Max']))));
        set(slider, 'Value', x);
        userdata.CallBack(slider);
    end


    function guiCloseFcn(src,evt)
        try
            closeConnection();
        end
        delete(src);
        disp('Goodbye!');
    end
      






%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%  REAL FUNCTIONS BEGIN HERE  %%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%  FUNCTION TO BEGIN (CALLED WHEN PRESSING START)  %%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function err = beginRoutine()
        err=setConnection();
        pid.plot.timer = timer(...
            'TimerFcn',@plotter,...
            'Name','Plotter Timer',...
            'Period',0.1,...
            'BusyMode','queue',...
            'ExecutionMode','fixedDelay'); % 'ErrorFcn',@errorFcn
        start(pid.plot.timer);
        if(err)
            return;
        end
        
        tic;
        pid.timerOn = 1;
        pid.time = toc;
        pid.timer = timer(...
            'TimerFcn',@serialHandler,...
            'Name','Serial Handler Timer',...
            'Period',1/pid.vars.freq,...
            'BusyMode','error',...
            'ExecutionMode','fixedDelay');%'BusyMode','drop',...
        start(pid.timer);
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  FUNCTION TO END  %%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function endRoutine()
        stop(pid.plot.timer);
        stop(pid.timer);
        pid.gui.timerOn = 0;
        fucked=1;
        while(fucked)
            try
                fwrite(pid.s.stream, 0, 'int16');
                fucked=0;
            end
        end
        closeConnection();
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%  SERIALHANDLER :: PERFORMS CONTROL LAW  %%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tic;
    function serialHandler(src,evt)
        % Read Position Values
        % Calculate
        % Output next PWM Values
        
        %shit=fgetl(pid.s.stream);
        %values = sscanf(shit,'%d,%d');
        %fwrite(pid.s.stream, pid.vals.outputVal, 'int16');
        %fwrite(pid.s.stream, 255, 'int8'); % Tell the MCU to talk.
        t=toc;
        [penduPos N msg] = fread(pid.s.stream, 1, 'int16');
        if(isequal(msg,'The specified amount of data was not returned within the Timeout period.'))
            penduPos = pid.vals.penduPos(1);
        end
        [motorPos N msg] = fread(pid.s.stream, 1, 'int16');
        if(isequal(msg,'The specified amount of data was not returned within the Timeout period.'))
            motorPos = pid.vals.motorPos(1);
        end
        motorPosDer = (motorPos - pid.vals.motorPos(1)) * pid.vars.freq;
        %motorPosDer = 1.05 * motorPosDer - 0.05 * pid.vals.motorPosDer(1);
        motorAcc = (motorPosDer - pid.vals.motorPosDer(1)) * pid.vars.freq;
        
        ref = 0;
        pid.vals.penduPos = [penduPos, pid.vals.penduPos(1)];
        pid.vals.motorPos = [motorPos, pid.vals.motorPos(1)];
        pid.vals.motorPosDer = [motorPosDer, pid.vals.motorPosDer(1)];
        pid.vals.motorAcc = [motorAcc, pid.vals.motorAcc(1)];
        %penduPosInt = pid.vals.penduPosInt(1)+penduPos/pid.vars.freq;
        %pid.vals.penduPosInt = [penduPosInt, pid.vals.penduPosInt(1)];
        err = ref - mod(penduPos, 1024) + 512;
        
        errInt = pid.vals.errInt(1) + err/pid.vars.freq;
        errDer = (err - pid.vals.err(1))*pid.vars.freq;
        
        pid.vals.err = [err, pid.vals.err(1)];
        if err == 0
            errInt = 0;
        end
        pid.vals.errInt = [errInt, pid.vals.errInt(1)];
        pid.vals.errDer = [errDer, pid.vals.errDer(1)];
        errAcc = (pid.vals.errDer(1) - pid.vals.errDer(2)) * pid.vars.freq;
        
        acc = 0.00;%5;
        
        
        if ((err>0 && errInt > 0) || (err<0 && errInt < 0))
            derGainAdjust = 0.9;
        else
            derGainAdjust = 1;
        end
        derGain = pid.vars.der/100 * derGainAdjust;
        
        angleFactor = 1+abs(tan(err/256*pi/1.4));% exp(abs(err-errDer/25)/256/2.5); %(1+tan(abs(err+errDer/256)/256*pi/1.5)/2); % tan(abs(err)/256*pi);
        derFactor = 1 + 1/pid.vals.outputVal(1);
        pidVal = pid.vars.pos/100 * tan(err/256*pi/1.2) * 256 + ...
                 pid.vars.int/100 * errInt + ...
                 pid.vars.der/100 * errDer * derFactor + ...
                 acc/100 * errAcc + ...
                 0.000005/100 * motorPosDer * abs(motorPosDer)^0.6 - ...
                 0.00005/100 * motorAcc; % Very Simple Control Law
             
                 %pid.vars.der/100 * errDer + ...
             %  + ...
             % * tan(err/256*pi/2) + ...
             
        %if(size(pidVal) ~= [1 1])
        %    pidVal = 0;
        %end
        
        sign = 2*(motorPosDer>0)-1;
        index = min(256,round(abs(motorPosDer)/255+1));
        if index==1
            if pidVal>0
                sign = 1;
            elseif pidVal<0
                sign = -1;
            else
                sign = 0;
            end
        end
        backtorque = pid.torques(index)/255;
        pid.vals.backtorque = backtorque;
        t = pidVal+sign*backtorque;
        pwm = min(255,max(-255,t*91.74));
        
        outputVal = pwm;
        outputVal = 0.5*pwm+0.5*pid.vals.outputVal(1);
        pid.vals.outputVal(2) = pid.vals.outputVal(1);
        pid.vals.outputVal(1) = outputVal;
        %penduPosDer = penduPos - pid.vals.penduPos(2);
        %pid.vals.penduPosDer = [penduPosDer, pid.vals.penduPosDers(1)];
        %set(pid.gui.penduPosText, 'String', {num2str(penduPos), num2str(outputVal)});
        if (abs(err)>110)
            outputVal = 0;%getitup();
            pid.vals.errInt(1) = 0;
        end
        try
            fwrite(pid.s.stream, outputVal, 'int16');
            % min(255,max(-255,outputVal))
        catch
            %disp('fuck!');
        end
        pid.vals.time = toc - t;
    end

    % Function to "get it up"
    function val = getitup()
        if pid.vals.penduPos(1)-pid.vals.penduPos(2) == 0
            sign = 2*(pid.vals.err(1)>0)-1;
            val = sign*150;
        elseif abs(pid.vals.err(1)) > 480
            sign = 2*(pid.vals.penduPos(1)-pid.vals.penduPos(2)<0)-1;
            val = sign*180;
        else
            try
                val = 0;
            end
        end
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%  PLOTTER :: PLOTS VALUES ON DISPLAY  %%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

    pid.plot.numVals = 2;
    pid.plot.xlength = 50;
    pid.plot.x = 1:pid.plot.xlength;
    pid.plot.y = ones(pid.plot.numVals, pid.plot.xlength)*NaN;
    pid.plot.plot = plot(pid.plot.x,pid.plot.y,'LineWidth',2);
    pid.plot.index = 1;

    function plotter(fuck,this,shit)
        values = [pid.vals.err(1); pid.vals.outputVal(1)];
        %plotScroller(pid.plot, ...
        %    struct('Values',[pid.vals.penduPos, pid.vals.outputVal]));
        
        pid.plot.y(:,pid.plot.index) = values;
        for (k=1:pid.plot.numVals)
            set(pid.plot.plot(k), 'ydata', pid.plot.y(k,[[pid.plot.index+1:end] [1:pid.plot.index]]));
        end
        pid.plot.index = mod(pid.plot.index, pid.plot.xlength)+1;
        
        set(pid.gui.penduPosText, 'String', {['Error: ' num2str(pid.vals.err(1))],['Output:' num2str(pid.vals.outputVal(1))]});
    end
        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%  SETCONNECTION :: SETS UP USART CONNECTION  %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function err=setConnection()
        try
            pid.s.stream=serial(['COM' num2str(pid.s.port)],...
                                'BaudRate',pid.s.baudRate,...
                                'ByteOrder','BigEndian',...
                                'Timeout',0.05);%,...
                                %'Terminator','CR');
            %disp(pid.s.stream);
            fopen(pid.s.stream);
            %fgetl(pid.s.stream);
            %set(pid.s.stream, 'BytesAvailableFcn', @serialHandler);
            err = 0;
        catch
            errordlg('COM Port Not Available');
            err = 1;
        end
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  CLOSECONNECTION :: CLOSES USART CONNECTION  %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function closeConnection()
        fclose(pid.s.stream);
    end
        

    
end